#include "qmi8658c.h" // 包含 QMI8658C 传感器驱动头文件
#include "driver/i2c.h"     // 包含I2C驱动程序的头文件
#include "esp_log.h"            // 包含日志相关的头文件 
#include "myi2c.h"          // 包含自定义的I2C初始化函数
#include "freertos/FreeRTOS.h"  // 包含 FreeRTOS 头文件
#include "freertos/task.h"      // 包含任务相关的头文件
#include <math.h>           //  包含数学函数库头文件

static const char *标签 = "QMI8658C";    
// 加速度计和陀螺仪的校准值
esp_err_t qmi8658c_register_read(uint8_t 寄存器地址, uint8_t *数据, size_t 长度)
{
    return i2c_master_write_read_device(I2C主端口, QMI8658C传感器地址,  &寄存器地址, 1, 数据, 长度, I2C自动超时 / portTICK_PERIOD_MS);
}

// 写一个字节到 QMI8658C 寄存器
esp_err_t qmi8658c_register_write_byte(uint8_t 寄存器地址, uint8_t 数据)
{
    uint8_t write_buf[2] = {寄存器地址, 数据};

    return i2c_master_write_to_device(I2C主端口, QMI8658C传感器地址, write_buf, sizeof(write_buf), I2C自动超时 / portTICK_PERIOD_MS);
}

// 初始化 QMI8658C 传感器
void qmi8658c_init(void)
{
    uint8_t id = 0;

    qmi8658c_register_read(QMI8658C我是谁, &id ,1);     // 读 ID
    while (id != 0x05)
    {
        vTaskDelay(1000 / portTICK_PERIOD_MS);
        qmi8658c_register_read(QMI8658C我是谁, &id ,1);// 读 ID
    }
    ESP_LOGI(标签, "QMI8658C OK!");

    qmi8658c_register_write_byte(QMI8658C复位, 0xb0);  // 复位
    vTaskDelay(10 / portTICK_PERIOD_MS);               // 延时
    qmi8658c_register_write_byte(QMI8658C控制1, 0x40); // CTRL1 设置地址自动增加
    qmi8658c_register_write_byte(QMI8658C控制7, 0x03); // CTRL7 允许加速器和陀螺仪
    qmi8658c_register_write_byte(QMI8658C控制2, 0x95); // CTRL2 设置ACC 4g 250Hz
    qmi8658c_register_write_byte(QMI8658C控制3, 0xd5); // CTRL3 设置GRY 512dps 250Hz
}

void qmi8658c_Read_AccAndGry(t_sQMI8658C *p)            // 读加速器和陀螺仪
{
    uint8_t 状态, 数据就绪=0;
    int16_t buf[6];

    qmi8658c_register_read(QMI8658C状态0, &状态, 1);    // 读状态寄存器
    if (状态 & 0x03)                                    // 判断加速器和陀螺仪数据是否可读
    {
        数据就绪 = 1;                                   // 数据就绪标志置为1
    }
    if (数据就绪 == 1)                                   // 如果数据就绪
    {
        数据就绪 = 0;                                   // 数据就绪标志清零
        qmi8658c_register_read(QMI8658C_AX_L, (uint8_t *)buf, 12); // 读加速器值
        p->加速器x = buf[0];                           
        p->加速器y = buf[1];
        p->加速器z = buf[2];
        p->陀螺仪x = buf[3];
        p->陀螺仪y = buf[4];
        p->陀螺仪z = buf[5];
    }
}

void qmi8658c_fetch_angleFromAcc(t_sQMI8658C *p)        // 从加速器计算角度
{
    float temp;

    qmi8658c_Read_AccAndGry(p);                         // 读加速器值   

    temp = (float)p->加速器x / sqrt( ((float)p->加速器y * (float)p->加速器y + (float)p->加速器z * (float)p->加速器z) );
    p->角度X = atan(temp)*57.3f;                        // 180/3.14=57.3
    temp = (float)p->加速器y / sqrt( ((float)p->加速器x * (float)p->加速器x + (float)p->加速器z * (float)p->加速器z) );
    p->角度Y = atan(temp)*57.3f;                        // 180/3.14=57.3
    temp = (float)p->加速器z / sqrt( ((float)p->加速器x * (float)p->加速器x + (float)p->加速器y * (float)p->加速器y) );
    p->角度Z = atan(temp)*57.3f;                        // 180/3.14=57.3
}   